#include "include.h"
#include "math.h"
#include "model.h"
#include <stdio.h>

int object = 0;//对象类型，默认为水果
int MODE = 0;

float targetX = 6, targetY = 0;//for mode4
float lastX = 0.2, lastY = 0.0;

int armstart = 1;//舵机状态，1-确定动作 2-清除动作位

int mode4_time = 750;//单个动作时间
int Delay_time = 700;//延迟时间，用于完成动作

//左、右、前
#define L  2225
#define LM 1940
#define R  930
#define RM 1150
#define M  1570

//等待当前动作完成
void Wait_ArmAction()
{
	int i =0;
	for(i=1;i<7;i++)
	{
		while(ServoPwmDuty[i] != ServoPwmDutySet[i]);
		//DelayMs(Delay_time);
	}

}

int servo_limit(int pwm)
{
	if(pwm<500)pwm = 500;
	else if(pwm>1500)pwm = 1500;
	return pwm;
}
//AB区抓取动作（L、R）
void Grap_AB(uint16 High)
{
	switch(High)
	{
		case 0:	//地上
			ServoSetPluseAndTime(1, 500, mode4_time);
			ServoSetPluseAndTime(3, 1500, mode4_time);
			ServoSetPluseAndTime(4, 2123, mode4_time);
			ServoSetPluseAndTime(5, 2400, mode4_time);
			Wait_ArmAction();	//等待动作完成			
			ServoSetPluseAndTime(1, 2000, mode4_time);
			Wait_ArmAction();	//等待动作完成
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1540, mode4_time);
			ServoSetPluseAndTime(3, 800, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;
		case 1: //树上
			ServoSetPluseAndTime(1, 500, mode4_time);
			ServoSetPluseAndTime(5, 1600, mode4_time);
			ServoSetPluseAndTime(4, 1900, mode4_time);
			ServoSetPluseAndTime(3, 1250, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;
		case 2: //C地面
			ServoSetPluseAndTime(1, 500, mode4_time);
			ServoSetPluseAndTime(5, 2200, mode4_time);
			ServoSetPluseAndTime(4, 2000, mode4_time);
			ServoSetPluseAndTime(3, 950, mode4_time);
			Wait_ArmAction();	//等待动作完成
			ServoSetPluseAndTime(1, 2000, mode4_time);
			Wait_ArmAction();	//等待动作完成
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1540, mode4_time);
			ServoSetPluseAndTime(3, 800, mode4_time);
			Wait_ArmAction();	//等待动作完成
			ServoSetPluseAndTime(6, M, mode4_time);
			Wait_ArmAction();
			break;
		case 3: //C上坡
			ServoSetPluseAndTime(1, 500, mode4_time);
			ServoSetPluseAndTime(5, 2250, mode4_time);
			ServoSetPluseAndTime(4, 2000, mode4_time);
			ServoSetPluseAndTime(3, 980, mode4_time);
			Wait_ArmAction();	//等待动作完成
			ServoSetPluseAndTime(1, 2000, mode4_time);
			Wait_ArmAction();	//等待动作完成
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1540, mode4_time);
			ServoSetPluseAndTime(3, 800, mode4_time);
			Wait_ArmAction();	//等待动作完成
			ServoSetPluseAndTime(6, M, mode4_time);
			Wait_ArmAction();
			break;
		default:
			break;
	}

}
//CD区抓取动作（F0、1、2、3、4、5、6）
void Grap_CD(uint16 High)
{
	switch(High)
	{
		case 0:	//地上
			ServoSetPluseAndTime(1, 500, mode4_time);
			ServoSetPluseAndTime(2, 1500, mode4_time);
			ServoSetPluseAndTime(3, 996, mode4_time);
			ServoSetPluseAndTime(4, 1653, mode4_time);
			ServoSetPluseAndTime(5, 2157, mode4_time);
			Wait_ArmAction();
		
			ServoSetPluseAndTime(1, 2000, mode4_time);
			ServoSetPluseAndTime(3, 908, mode4_time);
		  ServoSetPluseAndTime(4, 1540, mode4_time);
			ServoSetPluseAndTime(5, 1950, mode4_time);
			Wait_ArmAction();
		
			ServoSetPluseAndTime(5, 1500, mode4_time);
			Wait_ArmAction();
			break;
		case 1: 
			ServoSetPluseAndTime(1, 500, mode4_time);
			ServoSetPluseAndTime(2, 1500, mode4_time);
			ServoSetPluseAndTime(3, 846, mode4_time);
			ServoSetPluseAndTime(4, 1886, mode4_time);
			ServoSetPluseAndTime(5, 2339, mode4_time);
			Wait_ArmAction();	 //等待动作完成	
		
			ServoSetPluseAndTime(1, 2000, mode4_time);
			ServoSetPluseAndTime(3, 800, mode4_time);
		  ServoSetPluseAndTime(4, 1790, mode4_time);
			ServoSetPluseAndTime(5, 2150, mode4_time);

			Wait_ArmAction();	 //等待动作完成	
		
			ServoSetPluseAndTime(5, 1500, mode4_time);
			Wait_ArmAction();  //等待动作完成
			break;
		case 2: //25cm
			ServoSetPluseAndTime(3, 2100, mode4_time);
			ServoSetPluseAndTime(4, 1300, mode4_time);
			ServoSetPluseAndTime(5, 1800, mode4_time);
			Wait_ArmAction();	 //等待动作完成				
			break;
		case 3: //30cm
			ServoSetPluseAndTime(3, 2037, mode4_time);
			ServoSetPluseAndTime(4, 1293, mode4_time);
			ServoSetPluseAndTime(5, 1700, mode4_time);
			Wait_ArmAction();	 //等待动作完成				
			break;
		case 4: //35cm 
			ServoSetPluseAndTime(3, 1800, mode4_time);
			ServoSetPluseAndTime(4, 1450, mode4_time);
			ServoSetPluseAndTime(5, 1600, mode4_time);
			Wait_ArmAction();	 //等待动作完成				
			break;
		case 5: //40cm
			ServoSetPluseAndTime(3, 1625, mode4_time);
			ServoSetPluseAndTime(4, 1616, mode4_time);
			ServoSetPluseAndTime(5, 1600, mode4_time);
			Wait_ArmAction();	 //等待动作完成				
			break;
		case 6: //45cm
			ServoSetPluseAndTime(3, 1350, mode4_time);
			ServoSetPluseAndTime(4, 1900, mode4_time);
			ServoSetPluseAndTime(5, 1650, mode4_time);
			Wait_ArmAction();	 //等待动作完成				
			break;
		case 7: //50cm
			ServoSetPluseAndTime(3, 850, mode4_time);
			ServoSetPluseAndTime(4, 2400, mode4_time);
			ServoSetPluseAndTime(5, 1700, mode4_time);
			Wait_ArmAction();	 //等待动作完成			
			ServoSetPluseAndTime(1, 2000, mode4_time);
			Wait_ArmAction();	//等待动作完成
			ServoSetPluseAndTime(3, ServoPwmDutySet[3]+200,mode4_time);
			ServoSetPluseAndTime(5, ServoPwmDutySet[5]-100,mode4_time);		
			Wait_ArmAction();	//等待动作完成
			break;
		default:
			break;
	}
}
//收集水果
void Collect(uint16 High)
{
//	int i =0;
	ServoSetPluseAndTime(1, 2000, mode4_time);
	Wait_ArmAction();	//等待动作完成
  switch(High)
	{
		case 0:	//地上
			ServoSetPluseAndTime(3, 2280, mode4_time);
			ServoSetPluseAndTime(4, 2400, mode4_time);
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(6, M, mode4_time);
			Wait_ArmAction();
			break;
		case 1: //靶上
			//C靶上拿下
			if(ServoPwmDutySet[5]-300<1500) ServoPwmDutySet[5]=1800;
			ServoSetPluseAndTime(5, ServoPwmDutySet[5]-300, mode4_time);
			ServoSetPluseAndTime(3, ServoPwmDutySet[3]+100, mode4_time);
			ServoSetPluseAndTime(4, ServoPwmDutySet[4]+150, mode4_time);
			Wait_ArmAction();
		
			ServoSetPluseAndTime(3, 2280, mode4_time);
			ServoSetPluseAndTime(4, 2400, mode4_time);
			ServoSetPluseAndTime(5, 1500, mode4_time);
		  ServoSetPluseAndTime(6, M, mode4_time);
			Wait_ArmAction();
			break;
		case 2: //C地面
			ServoSetPluseAndTime(3, 2280, mode4_time);
			ServoSetPluseAndTime(4, 2400, mode4_time);
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(6, M, mode4_time);
			Wait_ArmAction();
			break;
		case 3: //C上坡
			ServoSetPluseAndTime(3, 2280, mode4_time);
			ServoSetPluseAndTime(4, 2400, mode4_time);
			ServoSetPluseAndTime(5, 1500, mode4_time);
			Wait_ArmAction();
			ServoSetPluseAndTime(6, M, mode4_time);
			Wait_ArmAction();
			break;
		case 4: //D树上
			ServoSetPluseAndTime(5, ServoPwmDutySet[5]+100, mode4_time);
			ServoSetPluseAndTime(3, ServoPwmDutySet[3]-100, mode4_time);
			ServoSetPluseAndTime(4, ServoPwmDutySet[4]-150, mode4_time);
			Wait_ArmAction();
		
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1900, mode4_time);
			ServoSetPluseAndTime(3, 567, mode4_time);
		  Wait_ArmAction();	//等待动作完成			
		
			break;
	}
	//张爪
	if(High!=4)ServoSetPluseAndTime(1, 500, mode4_time);
	Wait_ArmAction();	//等待动作完成
}
//默认动作，向前看
void Ready()
{
	ServoSetPluseAndTime(6, M, mode4_time);
	ServoSetPluseAndTime(5, 1500, mode4_time);
	ServoSetPluseAndTime(4, 1900, mode4_time);
	ServoSetPluseAndTime(3, 567, mode4_time);
	ServoSetPluseAndTime(2, 1500, mode4_time);
	ServoSetPluseAndTime(1, 1000, mode4_time);
	Wait_ArmAction();	//等待动作完成
}
//向前看 （K0、1、2、3、4、5、6）
void LookDown(uint16 distance)
{
	ServoSetPluseAndTime(1, 1000, mode4_time);
	switch(distance)
	{
		case 0:	//近处
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1429, mode4_time);
			ServoSetPluseAndTime(3, 567, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;
		case 1: //远处
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1900, mode4_time);
			ServoSetPluseAndTime(3, 567, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;
		case 2: 
			ServoSetPluseAndTime(5, 1100, mode4_time);
			ServoSetPluseAndTime(4, 1115, mode4_time);
			ServoSetPluseAndTime(3, 1623, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;		
		case 3: 
			ServoSetPluseAndTime(5, 1100, mode4_time);
			ServoSetPluseAndTime(4, 806, mode4_time);
			ServoSetPluseAndTime(3, 1914, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;			
	}
	
}
//B区识别成熟、B到C区（A、D01234）
void Look(uint16 High)
{
	ServoSetPluseAndTime(1, 1000, mode4_time);
	switch(High)
	{
		case 0:	//地上
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1771, mode4_time);
			ServoSetPluseAndTime(3, 586, mode4_time);
			Wait_ArmAction();
			break;
		case 1: //树上
			ServoSetPluseAndTime(5, 1200, mode4_time);
			ServoSetPluseAndTime(4, 1300, mode4_time);
			ServoSetPluseAndTime(3, 1500, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;
		case 2: //远看一米十字
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 2030, mode4_time);
			ServoSetPluseAndTime(3, 567, mode4_time);
			Wait_ArmAction(); //等待动作完成
			break;
		case 3: //近看一米十字
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1908, mode4_time);
			ServoSetPluseAndTime(3, 567, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;
		case 4: //看C区中间十字
			ServoSetPluseAndTime(5, 1500, mode4_time);
			ServoSetPluseAndTime(4, 1700, mode4_time);
			ServoSetPluseAndTime(3, 567, mode4_time);
			Wait_ArmAction();	//等待动作完成
			break;
		default:
			break;
		
	}
	
}
//水平旋转
void Turn_Horizontal(uint16 Pulse)
{
	ServoSetPluseAndTime(6, Pulse, mode4_time);
	Wait_ArmAction();//等待动作完成
}
void clear_flag(void)
{
	L_state = -1;
	R_state = -1;
	F_state = G_state = g_state =  -1;							  
	Look_state = -1;
	L_GrapLook_state = -1;
	R_GrapLook_state = -1;
	Horizon_state = -1;
}

void mode4(float x,float y)
{
//	int i =0;
	switch(armstart)
	{
		case 0:			//保持动作
			break;
		case 1:			//确定动作
		{
			if(L_state != -1)			//AB区：抓取左边 高/低
			{
				if(L_state == 2||L_state == 3) Turn_Horizontal(LM);
				else	Turn_Horizontal(L);
				Grap_AB(L_state);
				Collect(L_state);
				Ready();
			}
			else if(R_state != -1)//AB区：抓取右边 高/低
			{
				if(R_state == 2||R_state == 3) Turn_Horizontal(RM);
				else	Turn_Horizontal(R);
				Grap_AB(R_state);
				Collect(R_state);
				Ready();
			}	
			else if(L_GrapLook_state != -1)   //B区：识别左边成熟度 1-树上 0-地上
			{	   		
				Look(L_GrapLook_state);
				if (L_GrapLook_state==4) Turn_Horizontal(LM);
				else Turn_Horizontal(L);																
			}
			else if(R_GrapLook_state != -1)		//B区：识别右边成熟度 1-树上 0-地上	
			{
				Look(R_GrapLook_state);
				if (R_GrapLook_state==4) Turn_Horizontal(RM);
				else Turn_Horizontal(R);							
			}	
			else if(G_state != -1)						
			{
				if(G_state<2)					//旧C区：抓取地面
					Turn_Horizontal(M);
				//34567不同高度抓取动作
				Grap_CD(G_state);
				if(G_state<2)
				{
					Ready();
					Collect(G_state);
				}
				else
					Collect(4);//不同高度抓取时的回收动作
			}			
			else if(F_state != -1)						
			{
				if(F_state<2)					//旧C区：抓取地面
					Turn_Horizontal(M);
				//34567不同高度抓取动作
				Grap_CD(F_state);
				if(F_state<2)
					Collect(F_state);
				else
					Collect(1);//不同高度抓取时的回收动作
				Ready();
			}
			else if(Look_state != -1)					//区域变换，向前看找十字，0-近处 1-远处
			{
				if(Look_state<4)
				{
					Turn_Horizontal(M);
					LookDown(Look_state);						//舵机46
				}
				else
				{
					LookDown(Look_state-2);						//舵机46
				}
			}
			else if(Horizon_state != -1)
			{
				if(Horizon_state==0) Turn_Horizontal(R);
				else if(Horizon_state==90) Turn_Horizontal(M);
				else if(Horizon_state==180) Turn_Horizontal(L);
				else Turn_Horizontal(R+Horizon_state*(L-R)/180);
			}
			else if(g_state != -1)
			{
				Collect(2);
				Ready();
			}
			else 	Ready();
			//u3_printf("%c%c%.2f\r\n",'~', 'S', 1.0);
			armstart = 0;
			clear_flag();//清除动作标志
			break;
		}
		case 2:
			u3_printf("%c%c%.2f\r\n",'~', 'S', 1.0);
			armstart = 0;			
			break;
	}
	
}
